#include "sky_seacher.h"

#include "elog.h"
#include "elog_file.h"
#include "timer_utils.h"
#include "ws_constant.h"

int SkySeacher::start(std::vector<HorizontalCoordinates>& calibration_path) {
    // 设置广角参数
    setCamWide();

    // 电机复位，旋转从限位开始，俯仰转到90度
    // motor_yaw_.resetAndSetParameterInDegree(YAW_LEFT, 0, YAW_RIGHT, STEP_MOTOR_NOW_POSITION);
    motor_yaw_.resetAndSetParameterInDegree();
    motor_yaw_.run();
    // motor_pitch_.resetAndSetParameterInDegree(PITCH_DOWN, motor_pitch_reset_coord_to_mechanical_coord, PITCH_UP, STEP_MOTOR_NOW_POSITION);
    motor_pitch_.resetAndSetParameterInDegree();
    motor_pitch_.run();
    motor_yaw_.waitMotorToStop();
    motor_pitch_.waitMotorToStop();

    HorizontalCoordinates center_mechanical_coord_wide_view;
    int ret = motor_yaw_.getAbsolutePositionInDegree(motor_resolution_level_, center_mechanical_coord_wide_view.azi);
    if (ret < 0)
        return -1;
    
    double alt = 0;
    ret = motor_pitch_.getAbsolutePositionInDegree(motor_resolution_level_, alt);
    if (ret < 0)
        return -1;

    alt -= motor_pitch_reset_coord_to_mechanical_coord;
    double yaw_angle = RADIANS_TO_DEGREES(acos((COSD(cam_wide_width_fov) - SIND(alt) * SIND(alt)) / (COSD(alt) * COSD(alt))));

    center_mechanical_coord_wide_view.alt = alt;

    bool direction = YAW_RIGHT;

    time_checker timer;

    while(1) {
        sleep(exposure_ * 2);

        cv::Mat img;
        cam_wide_.getJpgDecodeMat(img);
        // img = cv::imread("/userdata/sky_ground/data/DWARF_WIDI_1970-01-01-08-00-15-355.jpg");

        cv::Mat img_sky_mask;
        sky_seger_.start(img, img_sky_mask);

        // cv::Mat matMean;
        // cv::meanStdDev(img_sky_mask, matMean, cv::Mat());
        // double mean = matMean.at<double>(0,0) / 255.0;

        // if (mean > 0.5) {
            timer.start();
            ret = testAllTeleViewInWideView(img_sky_mask, center_mechanical_coord_wide_view, calibration_path);
            timer.stop();
            timer.show_distance("search sky cost");

            cv::imwrite("/tmp/sky_search.jpg", img_sky_mask);

            if (ret == 0) 
                return 0;
        // }

        motor_yaw_.setParameterInDegree(yaw_angle, motor_speed_, motor_resolution_level_, motor_speed_ramping_, direction);
        motor_yaw_.run();
        ret = motor_yaw_.waitMotorToStop();

        motor_yaw_.getAbsolutePositionInDegree(motor_resolution_level_, center_mechanical_coord_wide_view.azi);

        if (ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {
            direction = !direction;

            motor_pitch_.setParameterInDegree(cam_wide_height_fov, motor_speed_, motor_resolution_level_, motor_speed_ramping_, PITCH_UP);
            motor_pitch_.run();
            motor_pitch_.waitMotorToStop();

            motor_pitch_.getAbsolutePositionInDegree(motor_resolution_level_, alt);
            alt -= motor_pitch_reset_coord_to_mechanical_coord;
            if (alt >= 80) 
                return -1;
            yaw_angle = RADIANS_TO_DEGREES(acos((COSD(cam_wide_width_fov) - SIND(alt) * SIND(alt)) / (COSD(alt) * COSD(alt))));

            center_mechanical_coord_wide_view.alt = alt;
        }
    }
}

int SkySeacher::setCamWide() {
    // 开广角，调参数
    std::thread cam_wide_open([&]() {
        cam_wide_.openCamera(0); 
    });
    cam_wide_open.detach();
    usleep(1 * 1000);// 等待打开摄像头上锁
    cam_wide_.setExpMode(1);
    cam_wide_.setExp(exposure_);
    cam_wide_.setGain(gain_);

    return 0;
}

int SkySeacher::testAllTeleViewInWideView(cv::Mat& img_sky_mask, const HorizontalCoordinates& center_mechanical_coord_wide_view, std::vector<HorizontalCoordinates>& calibration_path) {
    HorizontalCoordinates center_pix_tele_view = center_mechanical_coord_wide_view;

    while(1) {
        int ret = testSameAltTeleViewInWideView(img_sky_mask, center_mechanical_coord_wide_view, center_pix_tele_view, calibration_path);

        if (ret == -2)
            break;
        else if (ret == 0)
            return 0;

        center_pix_tele_view.alt -= cam_tele_height_fov;// 先向下扫描
        if (center_pix_tele_view.alt < -90)
            break;
    }

    center_pix_tele_view = center_mechanical_coord_wide_view;

    while(1) {
        center_pix_tele_view.alt += cam_tele_height_fov;// 再向上扫描
        if (center_pix_tele_view.alt > 90)
            break;

        int ret = testSameAltTeleViewInWideView(img_sky_mask, center_mechanical_coord_wide_view, center_pix_tele_view, calibration_path);

        if (ret == -2)
            break;
        else if (ret == 0)
            return 0;
    }

    return -1;
}

int SkySeacher::testSameAltTeleViewInWideView(cv::Mat& img_sky_mask, const HorizontalCoordinates& center_mechanical_coord_wide_view, const HorizontalCoordinates& center_mechanical_coord_tele_view, std::vector<HorizontalCoordinates>& calibration_path) {
    HorizontalCoordinates center_pix_tele_view = center_mechanical_coord_tele_view;
    std::vector<HorizontalCoordinates> path;

    while (1) {
        std::vector<cv::Point> tele_view_in_wide_view;
        int ret = getTeleViewInWideView(center_mechanical_coord_wide_view, center_pix_tele_view, tele_view_in_wide_view);
        if (ret < 0) {
            if (center_pix_tele_view.azi != center_mechanical_coord_tele_view.azi) 
                return -1;
            else
                return -2;
        }

        if (isTeleViewGood(img_sky_mask, tele_view_in_wide_view)) {
            HorizontalCoordinates motor_position;
            motor_position.azi = center_pix_tele_view.azi;
            motor_position.alt = center_pix_tele_view.alt + motor_pitch_reset_coord_to_mechanical_coord;

            path.push_back(motor_position);
        }

        if (center_pix_tele_view.azi != center_mechanical_coord_tele_view.azi) {
            std::vector<cv::Point> tele_view_in_wide_view_symmetry;

            for (const cv::Point& point : tele_view_in_wide_view) 
                tele_view_in_wide_view_symmetry.push_back(cv::Point(2 * cam_wide_center_pix_x_ - point.x, point.y));// 以广角中轴为中心，左右旋转相同度数的长焦视野是对称的，将右边的长焦视野对称到左边

            if (isTeleViewGood(img_sky_mask, tele_view_in_wide_view_symmetry)) {
                HorizontalCoordinates motor_position;
                motor_position.azi = 2 * center_mechanical_coord_tele_view.azi - center_pix_tele_view.azi;
                motor_position.alt = center_mechanical_coord_tele_view.alt + motor_pitch_reset_coord_to_mechanical_coord;

                path.push_back(motor_position);
            }
        }

        if (path.size() == 3) {
            calibration_path.assign(path.begin(), path.end());
            for (const auto& point : calibration_path) 
                std::cout << "calibration_path.azi: " << point.azi << ", calibration_path.alt: " << point.alt << std::endl;

            return 0;
        }

        center_pix_tele_view.azi += calibration_rotate_angle_;// 检索右半边的广角图，左边能用对称求出
        center_pix_tele_view.azi = LIMIT_DEGREES_TO_360(center_pix_tele_view.azi);
    }
}

bool SkySeacher::isTeleViewGood(cv::Mat& img_sky_mask, const std::vector<cv::Point>& tele_view) {
    double resize_para = img_sky_mask.cols / cam_wide_resolution_x_;
    std::vector<cv::Point> tele_view_resize;
    for (const auto& point : tele_view) 
        tele_view_resize.push_back(cv::Point(int(point.x * resize_para), int(point.y * resize_para)));

    cv::Rect bounding_rect = cv::boundingRect(tele_view_resize);

    if ((bounding_rect & cv::Rect(0, 0, img_sky_mask.cols, img_sky_mask.rows)) != bounding_rect)
        return false;

    cv::Mat img_tele_view = cv::Mat::zeros(img_sky_mask.size(), CV_8UC1);
    std::vector<std::vector<cv::Point>> polygons;
    polygons.push_back(tele_view_resize);
    cv::fillPoly(img_tele_view, polygons, cv::Scalar(255));

    cv::Mat result;
    cv::bitwise_and(img_sky_mask(bounding_rect), img_tele_view(bounding_rect), result);

    // 测试画斜矩形
    std::vector<std::vector<cv::Point>> contours;
    contours.push_back(tele_view_resize);
    cv::polylines(img_sky_mask, contours, true, cv::Scalar(125), 2);

    if (cv::countNonZero(img_tele_view) == cv::countNonZero(result))
        return true;
    else
        return false;
}

int SkySeacher::getTeleViewInWideView(const HorizontalCoordinates& center_mechanical_coord_wide_view, const HorizontalCoordinates& center_mechanical_coord_tele_view, std::vector<cv::Point>& points) {
    cv::Point up_left_tele_view(0, 0);
    cv::Point up_right_tele_view(cam_tele_resolution_x_, 0);
    cv::Point down_right_tele_view(cam_tele_resolution_x_, cam_tele_resolution_y_);
    cv::Point down_left_tele_view(0, cam_tele_resolution_y_);

    HorizontalCoordinates up_left_tele_view_mechanical_coord;
    HorizontalCoordinates up_right_tele_view_mechanical_coord;
    HorizontalCoordinates down_right_tele_view_mechanical_coord;
    HorizontalCoordinates down_left_tele_view_mechanical_coord;

    pixelCoordToMechanicalCoord(CAM_TELE, up_left_tele_view, center_mechanical_coord_tele_view, up_left_tele_view_mechanical_coord);
    pixelCoordToMechanicalCoord(CAM_TELE, up_right_tele_view, center_mechanical_coord_tele_view, up_right_tele_view_mechanical_coord);
    pixelCoordToMechanicalCoord(CAM_TELE, down_right_tele_view, center_mechanical_coord_tele_view, down_right_tele_view_mechanical_coord);
    pixelCoordToMechanicalCoord(CAM_TELE, down_left_tele_view, center_mechanical_coord_tele_view, down_left_tele_view_mechanical_coord);

    cv::Point up_left_tele_view_in_wide_view;
    cv::Point up_right_tele_view_in_wide_view;
    cv::Point down_right_tele_view_in_wide_view;
    cv::Point down_left_tele_view_in_wide_view;

    mechanicalCoordToPixelCoord(CAM_WIDE, up_left_tele_view_mechanical_coord, center_mechanical_coord_wide_view, up_left_tele_view_in_wide_view);
    mechanicalCoordToPixelCoord(CAM_WIDE, up_right_tele_view_mechanical_coord, center_mechanical_coord_wide_view, up_right_tele_view_in_wide_view);
    mechanicalCoordToPixelCoord(CAM_WIDE, down_right_tele_view_mechanical_coord, center_mechanical_coord_wide_view, down_right_tele_view_in_wide_view);
    mechanicalCoordToPixelCoord(CAM_WIDE, down_left_tele_view_mechanical_coord, center_mechanical_coord_wide_view, down_left_tele_view_in_wide_view);

    if (isPointOutOfWideView(up_left_tele_view_in_wide_view))
        return -1;
    
    if (isPointOutOfWideView(up_right_tele_view_in_wide_view))
        return -1;

    if (isPointOutOfWideView(down_right_tele_view_in_wide_view))
        return -1;

    if (isPointOutOfWideView(down_left_tele_view_in_wide_view))
        return -1;

    

    points.push_back(up_left_tele_view_in_wide_view);
    points.push_back(up_right_tele_view_in_wide_view);
    points.push_back(down_right_tele_view_in_wide_view);
    points.push_back(down_left_tele_view_in_wide_view);

    log_i("tele view");
    log_i("up_left.x:%d, up_left.y:%d", up_left_tele_view_in_wide_view.x, up_left_tele_view_in_wide_view.y);
    log_i("up_right.x:%d, up_right.y:%d", up_right_tele_view_in_wide_view.x, up_right_tele_view_in_wide_view.y);
    log_i("down_right.x:%d, down_right.y:%d", down_right_tele_view_in_wide_view.x, down_right_tele_view_in_wide_view.y);
    log_i("down_left.x:%d, down_left.y:%d", down_left_tele_view_in_wide_view.x, down_left_tele_view_in_wide_view.y);

    return 0;
}

bool SkySeacher::isPointOutOfWideView(cv::Point& point) {
    if (point.x >= 0 && point.x <= cam_wide_resolution_x_ && point.y >= 0 && point.y <= cam_wide_resolution_y_)
        return false;
    else 
        return true;
}

int SkySeacher::pixelCoordToMechanicalCoord(int cam_id, const cv::Point& pixel_coord, const HorizontalCoordinates& center_mechanical_coord, HorizontalCoordinates& mechanical_coord) {
    cv::Mat pixel_coord_vector = (cv::Mat_<double>(3, 1) << 1.0, pixel_coord.x, pixel_coord.y);

    cv::Mat camera_coord_vector;
    if (cam_id == CAM_WIDE)
        camera_coord_vector = cam_wide_matrix_pixel_to_camera_coord_.inv() * pixel_coord_vector;
    else if (cam_id == CAM_TELE) 
        camera_coord_vector = cam_tele_matrix_pixel_to_camera_coord_.inv() * pixel_coord_vector;

    // 相机坐标系下，图像边界的方位角和俯仰角都在(-π/2 <= atan() <= π/2)中
    double theta_camera_coord = atan(camera_coord_vector.at<double>(0, 1));
    double phi_camera_coord = atan(camera_coord_vector.at<double>(0, 2) * cos(theta_camera_coord));

    cv::Mat xyz_camera_coord = (cv::Mat_<double>(3, 1) << cos(phi_camera_coord) * cos(theta_camera_coord), 
                                                          cos(phi_camera_coord) * sin(theta_camera_coord), 
                                                          sin(phi_camera_coord));

    cv::Mat matrix_rotate_z = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.azi), SIND(center_mechanical_coord.azi), 0,
                                                        -SIND(center_mechanical_coord.azi), COSD(center_mechanical_coord.azi), 0,
                                                         0, 0, 1);

    cv::Mat matrix_rotate_y = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.alt), 0, SIND(center_mechanical_coord.alt),
                                                         0, 1, 0,
                                                        -SIND(center_mechanical_coord.alt), 0, COSD(center_mechanical_coord.alt));

    cv::Mat matrix_rotate = matrix_rotate_y * matrix_rotate_z;

    cv::Mat xyz_mechanical_coord = matrix_rotate.inv() * xyz_camera_coord;

    double r = cv::norm(xyz_mechanical_coord);//应该为1
    mechanical_coord.alt = RADIANS_TO_DEGREES(asin(xyz_mechanical_coord.at<double>(0, 2) / r));

    mechanical_coord.azi = RADIANS_TO_DEGREES(atan2(xyz_mechanical_coord.at<double>(0, 1), xyz_mechanical_coord.at<double>(0, 0)));
    mechanical_coord.azi = LIMIT_DEGREES_TO_360(mechanical_coord.azi);

    log_i("mechanical_coord:(%f, %f)", mechanical_coord.azi, mechanical_coord.alt);

    return 0;
}

int SkySeacher::mechanicalCoordToPixelCoord(int cam_id, const HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord, cv::Point& pixel_coord) {
    cv::Mat xyz_mechanical_coord = (cv::Mat_<double>(3, 1) << COSD(mechanical_coord.alt) * COSD(mechanical_coord.azi), 
                                                              COSD(mechanical_coord.alt) * SIND(mechanical_coord.azi), 
                                                              SIND(mechanical_coord.alt));

    cv::Mat matrix_rotate_z = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.azi), SIND(center_mechanical_coord.azi), 0,
                                                        -SIND(center_mechanical_coord.azi), COSD(center_mechanical_coord.azi), 0,
                                                         0, 0, 1);

    cv::Mat matrix_rotate_y = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.alt), 0, SIND(center_mechanical_coord.alt),
                                                         0, 1, 0,
                                                        -SIND(center_mechanical_coord.alt), 0, COSD(center_mechanical_coord.alt));

    cv::Mat matrix_rotate = matrix_rotate_y * matrix_rotate_z;

    cv::Mat xyz_camera_coord = matrix_rotate * xyz_mechanical_coord;
    
    cv::Mat pixel_coord_vector;
    if (cam_id == CAM_WIDE)
        pixel_coord_vector = cam_wide_matrix_pixel_to_camera_coord_ * xyz_camera_coord / xyz_camera_coord.at<double>(0, 0);
    else if (cam_id == CAM_TELE)
        pixel_coord_vector = cam_tele_matrix_pixel_to_camera_coord_ * xyz_camera_coord / xyz_camera_coord.at<double>(0, 0);

    pixel_coord.x = (int)pixel_coord_vector.at<double>(0, 1);
    pixel_coord.y = (int)pixel_coord_vector.at<double>(0, 2);

    std::cout << "pixel_coord_vector= " << std::endl << " " << pixel_coord_vector << std::endl << std::endl;

    return 0;
}

void SkySeacher::test() {
    // 电机复位，旋转从限位开始，俯仰转到90度
    motor_yaw_.resetAndSetParameterInDegree();
    motor_yaw_.run();
    motor_pitch_.resetAndSetParameterInDegree();
    motor_pitch_.run();
    motor_yaw_.waitMotorToStop();
    motor_pitch_.waitMotorToStop();

    HorizontalCoordinates center_mechanical_coord_wide_view;
    int ret = motor_yaw_.getAbsolutePositionInDegree(motor_resolution_level_, center_mechanical_coord_wide_view.azi);
    if (ret < 0)
        return;
    
    double alt = 0;
    ret = motor_pitch_.getAbsolutePositionInDegree(motor_resolution_level_, alt);
    if (ret < 0)
        return;

    alt -= motor_pitch_reset_coord_to_mechanical_coord;
    center_mechanical_coord_wide_view.alt = alt;

    log_i("center_mechanical_coord_wide_view:(%f, %f)", center_mechanical_coord_wide_view.azi, center_mechanical_coord_wide_view.alt);

    cv::Point point_wide_view(0, cam_wide_resolution_y_ / 2);
    HorizontalCoordinates point_wide_view_mechanical_coord;

    pixelCoordToMechanicalCoord(CAM_WIDE, point_wide_view, center_mechanical_coord_wide_view, point_wide_view_mechanical_coord);

    motor_yaw_.setParameterInPositionInDegree(point_wide_view_mechanical_coord.azi, motor_speed_, motor_resolution_level_, motor_speed_ramping_);
    motor_yaw_.run();

    motor_pitch_.setParameterInPositionInDegree(point_wide_view_mechanical_coord.alt + motor_pitch_reset_coord_to_mechanical_coord, motor_speed_, motor_resolution_level_, motor_speed_ramping_);
    motor_pitch_.run();
}
